/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "scene/include/rlvw.h"

#include <math.h>

#include <mutex>

#include <glog/logging.h>

#include "google/protobuf/text_format.h"
#include "scene/include/hv_in_map.h"
#include "scene/include/msg_verify.h"

#include "v2xpb-asn-bsm.pb.h"
#include "v2xpb-asn-map.pb.h"
#include "v2xpb-asn-position.pb.h"
#include "v2xpb-asn-spat.pb.h"

namespace v2x {
namespace scene {
bool Rlvw::GetRlvwActive() { return this->rlvw_active_; }
v2x::scene::RlvwLevel Rlvw::GetSeverityLevel() { return this->severity_level_; }
bool Rlvw::IsRlvw(const ::v2xpb::asn::Bsm& bsm,
                  const ::v2xpb::asn::MapLane& lane,
                  const ::v2xpb::asn::SpatIntersection& spat_intersection) {
  MsgVerify msg_verify;
  std::lock_guard<std::mutex> lock(lock_);
  const auto& pose = bsm.pos();
  const auto& heading = bsm.heading();
  LOG(INFO) << "hv's heading is " << heading;
  const auto& speed = bsm.speed();
  LOG(INFO) << "hv's speed is " << speed;
  const auto& positions = lane.positions();
  const auto& pose_size = lane.positions_size();
  double accelerate = 0;
  if (bsm.has_accel_set()) {
    const auto& accelerate_set = bsm.accel_set();
    if (accelerate_set.has_lon()) {
      accelerate = accelerate_set.lon();
    }
  }
  auto spat_phase = msg_verify.GetSpatPhaseId(spat_intersection, lane);
  if (!spat_phase) {
    LOG(ERROR) << "the spats of the lane are not same";
    return false;
  }
  LOG(INFO) << "hv's accelerate is " << accelerate;
  LOG(INFO) << "pose_size is " << pose_size;
  if (pose_size < 2) {
    LOG(ERROR) << "number of lane's position is less than 2, return false";
    return false;
  }
  double distance = CalculateDistance(pose, heading, speed, lane);
  std::vector<::v2xpb::asn::SpatPhaseState> spat_phase_vec;
  for (int i = 0; i < spat_phase->phase_state_size(); ++i) {
    spat_phase_vec.push_back(spat_phase->phase_state(i));
  }
  double speed_limitation = 100;
  if (lane.speed_limits_size() != 0) {
    if (lane.speed_limits(0).has_speed()) {
      speed_limitation = lane.speed_limits(0).speed();
    }
    for (int i = 1; i < lane.speed_limits_size(); ++i) {
      if (lane.speed_limits(i).has_speed()) {
        if (lane.speed_limits(i).speed() < speed_limitation) {
          speed_limitation = lane.speed_limits(i).speed();
        }
      }
    }
  }
  LOG(INFO) << "the speed_limitation is " << speed_limitation;
  CalculateRlvwLevel(spat_phase_vec, speed, accelerate, distance);
  return true;
}
double Rlvw::CalculateDistance(const ::v2xpb::asn::Position& pose,
                               double heading, double speed,
                               const ::v2xpb::asn::MapLane& lane) {
  int pose_size = lane.positions_size();
  double last_second_x = lane.positions(pose_size - 2).xyz().x();
  double last_second_y = lane.positions(pose_size - 2).xyz().y();
  double last_x = lane.positions(pose_size - 1).xyz().x();
  double last_y = lane.positions(pose_size - 1).xyz().y();
  double lane_stop_line_angle =
      std::atan2(last_y - last_second_y, last_x - last_second_x);
  double run_angle = lane_stop_line_angle - heading;
  if (run_angle < 0) {
    run_angle *= -1;
  }
  if (run_angle > M_PI) {
    run_angle = 2 * M_PI - run_angle;
  }
  double delta_x = last_x - pose.xyz().x();
  double delta_y = last_y - pose.xyz().y();
  double linear_distance = std::sqrt(delta_x * delta_x + delta_y * delta_y);
  double distance = linear_distance / (2 * std::sin(run_angle * 0.5));
  LOG(INFO) << "the distance between the stop line and hv is " << distance;
  return distance;
}

bool Rlvw::IsRlvwActive(
    double remaining_time,
    const std::vector<::v2xpb::asn::SpatPhaseState>& spat_phase_vec) {
  if (remaining_time < spat_phase_vec[0].timing_end()) {
    if (spat_phase_vec[0].color() == ::v2xpb::asn::SpatPhaseColor::COLOR_RED) {
      return true;
    } else {
      return false;
    }
  }
  remaining_time -= spat_phase_vec[0].timing_end();
  for (int i = 1; i < spat_phase_vec.size(); ++i) {
    if (remaining_time < spat_phase_vec[i].timing_duration()) {
      if (spat_phase_vec[i].color() ==
          ::v2xpb::asn::SpatPhaseColor::COLOR_RED) {
        return true;
      } else {
        return false;
      }
    } else {
      remaining_time -= spat_phase_vec[i].timing_duration();
    }
  }
  return false;
}
bool Rlvw::CalculateRlvwLevel(
    const std::vector<::v2xpb::asn::SpatPhaseState>& spat_phase_vec,
    double speed, double accelerate, double distance) {
  // time used to reach the stop line with current speed and accelerate
  double time_to_stop_line = -1;
  if (std::fabs(accelerate) > std::numeric_limits<double>::epsilon()) {
    time_to_stop_line = (std::sqrt(std::pow(speed, 2)) - speed) / accelerate;
  } else {
    if (std::fabs(speed) > std::numeric_limits<double>::epsilon()) {
      time_to_stop_line = distance / speed;
    }
  }
  // time left before the red spat end
  double time_to_red_end = -1;
  switch (spat_phase_vec[0].color()) {
    case ::v2xpb::asn::SpatPhaseColor::COLOR_PERMISSIVE_GREEN:
    case ::v2xpb::asn::SpatPhaseColor::COLOR_PROTECTED_GREEN:
      time_to_red_end = spat_phase_vec[0].timing_end();
      for (int i = 1; i < spat_phase_vec.size(); ++i) {
        time_to_red_end += spat_phase_vec[i].timing_duration();
        if (spat_phase_vec[i].color() ==
            ::v2xpb::asn::SpatPhaseColor::COLOR_RED) {
          break;
        }
      }
      break;
    case ::v2xpb::asn::SpatPhaseColor::COLOR_YELLOW:
    case ::v2xpb::asn::SpatPhaseColor::COLOR_FLASHING_YELLOW:
      time_to_red_end = spat_phase_vec[0].timing_end();
      for (int i = 1; i < spat_phase_vec.size(); ++i) {
        time_to_red_end += spat_phase_vec[i].timing_duration();
        if (spat_phase_vec[i].color() ==
            ::v2xpb::asn::SpatPhaseColor::COLOR_RED) {
          break;
        }
      }
      break;
    case ::v2xpb::asn::SpatPhaseColor::COLOR_RED:
      time_to_red_end = spat_phase_vec[0].timing_end();
      break;
    default:
      LOG(WARNING) << "got unknown spat color";
      break;
  }
  LOG(INFO) << "time hv reach the stop line is " << time_to_stop_line;
  LOG(INFO) << "time before the red spat end is " << time_to_red_end;
  if (std::fabs(time_to_stop_line + 1) <
      std::numeric_limits<double>::epsilon()) {
    LOG(ERROR) << "time to stop line is -1, return false";
    return false;
  }
  double spat_time_duration = 0;
  for (int i = 0; i < spat_phase_vec.size(); ++i) {
    spat_time_duration += spat_phase_vec[i].timing_duration();
  }
  double remaining_time = std::fmod(time_to_stop_line, spat_time_duration);
  // time left before the red spat end
  rlvw_active_ = IsRlvwActive(remaining_time, spat_phase_vec);
  if (rlvw_active_) {
    LOG(INFO) << "red light violation warning scene is active";
    if (time_to_stop_line > 10) {
      severity_level_ = v2x::scene::RlvwLevel::kSeverityNotify;
      LOG(INFO) << "the red light violation warning level is SEVERITY NOTIFY";
    } else {
      severity_level_ = v2x::scene::RlvwLevel::kSeverityWarning;
      LOG(INFO) << "the red light violation warning level is SEVERITY WARNING";
    }
  } else {
    severity_level_ = v2x::scene::RlvwLevel::kNoSeverity;
    LOG(INFO) << "red light violation warning scene is not active";
    LOG(INFO) << "the red light violation warning level is NO SEVERITY";
  }
  return true;
}
}  // namespace scene
}  // namespace v2x
